﻿using System;
using System.Runtime.InteropServices;

namespace PengSW.MathHelper
{
    public static class MathHelper
    {
        /// <summary>
        /// 用最小二乘法对给定的一组X向量和Y向量按照 Y = a + bX 进行最优线性拟合。
        /// 返回参数a和b和平均误差
        /// </summary>
        /// <param name="aInput">输入向量，第一维长度不能少于2（至少两组数据），第二维长度不能小于2（至少两个向量，计算只限前两个向量）</param>
        /// <param name="a">输出拟合参数a</param>
        /// <param name="b">输出拟合参数b</param>
        /// <returns>输出平均误差</returns>
        public static double LeastSquare(double[,] aInput, out double a, out double b)
        {
            if (aInput.GetLength(1) < 2) throw new ApplicationException("输入向量不能少于两维！");
            int n = aInput.GetLength(0);
            if (n < 2) throw new ApplicationException("输入向量第一维不能少于两条数据！");
            double sumx = 0.0, sumy = 0.0, sumxy = 0.0, sumx2 = 0.0;
            for (int i = 0; i < n; i++)
            {
                sumx += aInput[i, 0];
                sumy += aInput[i, 1];
                sumxy += aInput[i, 0] * aInput[i, 1];
                sumx2 += aInput[i, 0] * aInput[i, 0];
            }
            b = (n * sumxy - sumx * sumy) / (n * sumx2 - sumx * sumx);
            a = (sumy - b * sumx) / n;

            double dY = 0.0;
            for (int i = 0; i < n; i++)
            {
                dY += Math.Abs(aInput[i, 1] - (a + b * aInput[i, 0]));
            }
            return dY / n;
        }

        public static double LeastSquare(double[] x, double[] y, out double a, out double b, out double maxdY)
        {
            if (x.Length != y.Length) throw new ApplicationException("输入数据x和y的长度必须相同！");
            int n = x.Length;
            if (n < 2) throw new ApplicationException("输入不能少于两条数据！");
            double sumx = 0.0, sumy = 0.0, sumxy = 0.0, sumx2 = 0.0;
            for (int i = 0; i < n; i++)
            {
                sumx += x[i];
                sumy += y[i];
                sumxy += x[i] * y[i];
                sumx2 += x[i] * x[i];
            }
            b = (n * sumxy - sumx * sumy) / (n * sumx2 - sumx * sumx);
            a = (sumy - b * sumx) / n;

            double dY = 0.0, aMaxdY = 0.0;
            for (int i = 0; i < n; i++)
            {
                double adY = Math.Abs(y[i] - (a + b * x[i]));
                if (adY > aMaxdY) aMaxdY = adY;
                dY += adY;
            }
            maxdY = aMaxdY;
            return dY / n;
        }

        /// <summary>
        /// 用拉格朗日曲线插值对给定四个坐标点按照Y = a0 + a1*X + a2*X^2 + a3*X^3进行插值计算，
        /// 求出并返回a0, a1, a2, a3。
        /// 要求x0, x1, x2, x3各不相同
        /// </summary>
        public static double[] ThreeSectionLagrange(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3)
        {
            double a3 = (((y3 - y0) * (x1 - x0) - (y1 - y0) * (x3 - x0)) / (x1 - x0) / (x3 - x0) / (x3 - x1) - ((y2 - y0) * (x1 - x0) - (y1 - y0) * (x2 - x0)) / (x2 - x0) / (x1 - x0) / (x2 - x1)) / (x3 - x2);
            double a2 = ((y2 - y0) * (x1 - x0) - (y1 - y0) * (x2 - x0)) / (x2 - x0) / (x1 - x0) / (x2 - x1) - (x0 + x1 + x2) * a3;
            double a1 = (y1 - y0) / (x1 - x0) - (x0 + x1) * ((y2 - y0) * (x1 - x0) - (y1 - y0) * (x2 - x0)) / (x2 - x0) / (x1 - x0) / (x2 - x1) + (x0 + x1) * x2 * a3;
            double a0 = (x1 * y0 - x0 * y1) / (x1 - x0) + x0 * x1 * ((y2 - y0) * (x1 - x0) - (y1 - y0) * (x2 - x0)) / (x2 - x0) / (x1 - x0) / (x2 - x1) - x0 * x1 * x2 * a3;
            return new double[] { a0, a1, a2, a3 };
        }

        public static double[] ThreeSectionLagrange2(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3)
        {
            double[,] M = new double[,]
            {
                {1, x0, x0 * x0, x0 * x0 * x0, y0 },
                {1, x1, x1 * x1, x1 * x1 * x1, y1 },
                {1, x2, x2 * x2, x2 * x2 * x2, y2 },
                {1, x3, x3 * x3, x3 * x3 * x3, y3 },
            };

            for (int i = 1; i < 4; i++)
                for (int j = 0; j < 5; j++)
                    M[i, j] -= M[0, j];

            for (int i = 0; i < 4; i++)
            {
                if (i == 1) continue;
                double r = M[i, 1] / M[1, 1];
                for (int j = 1; j < 5; j++)
                    M[i, j] -= M[1, j] * r;
            }

            for (int i = 0; i < 4; i++)
            {
                if (i == 2) continue;
                double r = M[i, 2] / M[2, 2];
                for (int j = 2; j < 5; j++)
                    M[i, j] -= M[2, j] * r;
            }

            for (int i = 0; i < 4; i++)
            {
                if (i == 3) continue;
                double r = M[i, 3] / M[3, 3];
                for (int j = 3; j < 5; j++)
                    M[i, j] -= M[3, j] * r;
            }

            return new double[] { M[0, 4], M[1, 4] / M[1, 1], M[2, 4] / M[2, 2], M[3, 4] / M[3, 3] };
        }

        /// <summary>
        /// 用拉格朗日曲线插值对给定四个坐标点按照Y = a0 + a1*X + a2*X^2 + a3*X^3进行插值计算，
        /// 并求出x在插值函数中的对应y。
        /// 要求x0, x1, x2, x3各不相同
        /// </summary>
        public static double ThreeSectionLagrange(double x0, double y0, double x1, double y1, double x2, double y2, double x3, double y3, double x)
        {
            double[] a = ThreeSectionLagrange2(x0, y0, x1, y1, x2, y2, x3, y3);
            double y = a[0] + a[1] * x + a[2] * x * x + a[3] * x * x * x;
            return y;
        }

        public static double[] TwoSectionLagrange(double x0, double y0, double x1, double y1, double x2, double y2)
        {
            double a2 = ((y2 - y0) / (x2 - x0) - (y1 - y0) / (x1 - x0)) / (x2 - x1);
            double a1 = (y1 - y0) / (x1 - x0) - (x0 + x1) * a2;
            double a0 = y0 - x0 * (y1 - y0) / (x1 - x0) + x0 * x1 * a2;
            return new double[] { a0, a1, a2 };
        }

        public static double TwoSectionLagrange(double x0, double y0, double x1, double y1, double x2, double y2, double x)
        {
            double[] a = TwoSectionLagrange(x0, y0, x1, y1, x2, y2);
            double y = a[0] + a[1] * x + a[2] * x * x;
            return y;
        }

        /// <summary>
        /// 按线性关系，根据(x0, y0)-(x1, y1)计算x所对应的y值。
        /// 要求x0, x1不相等。
        /// </summary>
        /// <returns></returns>
        public static double Linear(double x0, double y0, double x1, double y1, double x)
        {
            return y0 + (x - x0) * (y1 - y0) / (x1 - x0);
        }
    }
}
